clear all;
close all;

ro=7/10;

Qk=[2;0;0];
Zk=[0;0;0];

T=0.001;
for k=1:1:30000
  
    time(k)=k*T;

    thd(k)=sin(0.5*k*T)+cos(k*T);
    dthd(k)=0.5*cos(0.5*k*T)-sin(k*T);   
    ddthd(k)=-0.25*sin(0.5*k*T)-cos(k*T);
    dddthd(k)=-0.125*cos(0.5*k*T)+sin(k*T);    
    th(k)=Qk(1);
    dth(k)=Qk(2);
    ddth(k)=Qk(3);
    nth(k)=Zk(1);
    ndth(k)=Zk(2);
    nddth(k)=Zk(3);
    
    if time(k)<=10
        dt(k)=0;
    elseif time(k)<=15 
        dt(k)=0.5*sin(2*pi*k*T);
    elseif time(k)<=25 
        dt(k)=30*sin(2*pi*k*T);
    else
        dt(k)=0;
    end
    D=dt(k);
    
    e1=th(k)-thd(k);
    e2=dth(k)-dthd(k);
    e3=ddth(k)-ddthd(k);

    alpha2=dthd(k)-e1;
    fin2=dthd(k)-alpha2;
    alpha3=-(e1+e2-ddthd(k));
    fin3=ddthd(k)-alpha3;
    T3=((alpha3-ddthd(k))^ro-(ddthd(k)-alpha3))/dddthd(k);

    ut=-(e2+D+4.2*e3-(-fin3^ro-fin3)/T3);
    U(k)=ut;

    tSpan = [0 T];
	[tt,Qq] = ode45(@(t,x) plant(t,x,ut,D),tSpan,Qk);
    Qk = Qq(length(Qq),:);

    tSpan = [0 T];
	[tt1,Zq] = ode45(@(t,x) obv(t,x,ut,Qk),tSpan,Zk);
    Zk = Zq(length(Zq),:);
end

figure(1);
plot(time,th,'k',time,thd,'r--');
xlabel('time(s)');ylabel('th');
legend('th','thd');
grid on;

figure(2);
plot(time,dth,'k',time,dthd,'r--');
xlabel('time(s)');ylabel('dth');
legend('dth','dthd');
grid on;

figure(3);
plot(time,U,'k');
xlabel('time(s)');ylabel('U');
legend('U');
grid on;
